//
// Created by ZhaoXiaoFei on 2022/7/21.
//

#ifndef VINS_ESTIMATOR_INITIAL_SFM_HPP
#define VINS_ESTIMATOR_INITIAL_SFM_HPP

#include <Eigen/Dense>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <glog/logging.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

struct SfmFeature{
    bool state;
    int id;
    std::vector<std::pair<int, Eigen::Vector2d>> observation;
    double position[3];
    double depth;
};

struct ReprojectionError{
    ReprojectionError(double u, double v) : observed_u(u), observed_v(v){

    }

    template <typename T>
    bool operator()(const T* const Q_cw , const T* const T_cw, const T* const position, T* residual) const {
    //e[0] = T(k_) - x[0] * y[0] + x[1] * y[1];
        T p[3];
        ceres::QuaternionRotatePoint(Q_cw, position, p);

        p[0] += T_cw[0];
        p[1] += T_cw[1];
        p[2] += T_cw[2];

        T xp = p[0] / p[2];
        T yp = p[1] / p[2];

        residual[0] = xp - T(observed_u);
        residual[1] = yp - T(observed_v);
        return true;
    }

    static ceres::CostFunction* Create(const double u, const double v){
        return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>(
                new ReprojectionError(u, v));
    }

    double observed_u;
    double observed_v;
};

struct ReprojectionError3D
{
    ReprojectionError3D(double observed_u, double observed_v)
            :observed_u(observed_u), observed_v(observed_v)
    {}

    template <typename T>
    bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const
    {
        T p[3];
        ceres::QuaternionRotatePoint(camera_R, point, p);
        p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2];
        T xp = p[0] / p[2];
        T yp = p[1] / p[2];
        residuals[0] = xp - T(observed_u);
        residuals[1] = yp - T(observed_v);
        return true;
    }

    static ceres::CostFunction* Create(const double observed_x,
                                       const double observed_y)
    {
        return (new ceres::AutoDiffCostFunction<
                ReprojectionError3D, 2, 4, 3, 3>(
                new ReprojectionError3D(observed_x,observed_y)));
    }

    double observed_u;
    double observed_v;
};


class GlobalSfm{
public:
    void triangulation(Eigen::Matrix<double, 3, 4>& pose1, Eigen::Vector2d& point1,
                       Eigen::Matrix<double, 3, 4>& pose2, Eigen::Vector2d& point2, Eigen::Vector3d& point_3d);
    void triangulationTwoFrame(int frame_i, Eigen::Matrix<double, 3, 4>& pose1,
                       int frame_j, Eigen::Matrix<double, 3, 4>& pose2, std::vector<SfmFeature>& sfmVec);
    bool solveByPnP(Eigen::Matrix3d& R, Eigen::Vector3d& T, int& i, std::vector<SfmFeature>& sfmVec);

    bool construct(int frame_num, Eigen::Quaterniond* Q, Eigen::Vector3d* T, int l,
                   const Eigen::Matrix3d& relativeR, const Eigen::Vector3d& relativeT, std::vector<SfmFeature>& sfmVec,
                   std::map<int, Eigen::Vector3d>& sfm_tracked_points);
};

#endif //VINS_ESTIMATOR_INITIAL_SFM_HPP
